/*==============================================================================
*
*                                   木目智能电子
*
*产品名称 :
*
*文件名称 : ModbusSlave.c
*软件作者 : Yummo
*修改记录 :新建    时间 : 2021年3月15日 下午9:22:26
==============================================================================*/
#include "cmsis_os2.h"
#include "usart.h"
#include "ModbusTable.h"
#include "ModbusSlave.h"

#define SLAVE_ADDR                (0x01)

#define FUNC_RD_COILS             (0x01)
#define FUNC_RD_DISC              (0x02)
#define FUNC_RD_HOLD_REG          (0x03)
#define FUNC_RD_INPUT_REG         (0x04)
#define FUNC_WR_SGL_COIL          (0x05)
#define FUNC_WR_SGL_REG           (0x06)
#define FUNC_WR_MULT_COILS        (0x0F)
#define FUNC_WR_MULT_REGS         (0x10)

#define CODE_ILGAL_DATA_ADR       (0x02)
#define CODE_ILGAL_DATA_VAL       (0x03)

#define MSG_INTERVAL_DELAY        (3) // 3.5bytes=28bits 28/9600=0.002916s ≈ 3ms

/* modbus PDU struct
typedef struct tag_Modbus_PDU
{
  uint8_t u8Func;
  uint8_t u8DataLen; //data长度,不包括功能码
  uint8_t *pu8Data;
}MbAdu_t;
*/
//- modbus ADU struct 
typedef struct tag_Modbus_ADU
{
  uint8_t u8Adr;
  struct tag_Modbus_PDU
  {
    uint8_t u8Func;
    uint8_t u8DataLen; //data长度,不包括功能码
    uint8_t *pu8Data;
  }PDU;
  uint16_t u16CRC; //低位在前,高位在后
}MbAdu_t;

//- ring buffer
typedef struct tag_Ring_Buffer
{
  uint8_t u8Head;
  uint8_t u8Tail;
  uint8_t u8Idx;
  uint8_t u8Buf[256]; 
}RB_t;

//-private code------------------------------------------------------------------
static uint8_t s_flgMbSlvRxOk(void);
static uint16_t s_u16CrcCal(MbAdu_t *pADU);
static void s_MsgUnpack(void);
static void s_MsgReqsonse(void);

//-private data------------------------------------------------------------------
static MbAdu_t s_MbMsgRx;
static MbAdu_t s_MbMsgTx;
static uint8_t s_u8Data[256];
static RB_t s_RxBuf;
static RB_t s_TxBuf;

//-public data-------------------------------------------------------------------
uint8_t g_flagModbusValChange=0;

void ISR_BufTx(UART_HandleTypeDef *huart);
void ISR_BufRx(UART_HandleTypeDef *huart);

/********************************************************************************
*函数功能: 
  注册发送和接收中断回调函数
*修改记录: 
*日期:                作者:                摘要:
      21-03-21             YummoShin            Create
********************************************************************************/
void InitMbSlv(void)
{
  s_MbMsgRx.PDU.pu8Data = s_u8Data;
  s_MbMsgTx.PDU.pu8Data = s_u8Data;
  huart6.TxISR = (void(*)(UART_HandleTypeDef *huart))ISR_BufTx; //注册发送中断回调函数
  huart6.RxISR = (void(*)(UART_HandleTypeDef *huart))ISR_BufRx; //注册接收中断回调函数
  ATOMIC_SET_BIT(huart6.Instance->CR1, USART_CR1_RXNEIE);
}

/********************************************************************************
*函数功能:
*修改记录:
*日期:                            作者:                      摘要:
      2021年3月15日      YummoShin
********************************************************************************/
void taskMbSlv(void)
{
  while(1)
  {
    if(s_flgMbSlvRxOk()) //接收到主机请求
    {
      s_MsgUnpack(); //解析主机请求
      s_MsgReqsonse(); //回复主机请求
    }
    osDelay(10);
  }
}

/********************************************************************************
*函数功能: 
  根据协议判断接收到完整帧
  @retval 1:接收到完整消息帧, 0:未接受完
*修改记录: 
*日期:                作者:                摘要:
      21-03-16             YummoShin            Create
********************************************************************************/
static uint8_t s_flgMbSlvRxOk(void)
{
  uint8_t flgMsgOk=0;
  uint8_t u8TailTmp=s_RxBuf.u8Tail;
  uint8_t u8Len, u8Data;

  while(u8TailTmp!=s_RxBuf.u8Idx)
  {
    u8Len = s_RxBuf.u8Idx-s_RxBuf.u8Head;
    u8Data = s_RxBuf.u8Buf[s_RxBuf.u8Idx];

    switch(u8Len)
    {
    case 0: //address
      if(u8Data!=SLAVE_ADDR)
        s_RxBuf.u8Head = s_RxBuf.u8Idx+1;
      else
        s_MbMsgRx.u8Adr = u8Data;
      break;

    case 1: //function code, HOLD REGISTER and INPUT REGISTER valid
      if(
          u8Data!=FUNC_RD_HOLD_REG  &&
          u8Data!=FUNC_RD_INPUT_REG &&
          u8Data!=FUNC_WR_SGL_REG   &&
          u8Data!=FUNC_WR_MULT_REGS
      )
        s_RxBuf.u8Head = s_RxBuf.u8Idx+1;
      else
        s_MbMsgRx.PDU.u8Func = u8Data;
      break;

    case 2: //handle by function
      switch(s_MbMsgRx.PDU.u8Func)
      {
      case FUNC_RD_HOLD_REG:      
      case FUNC_RD_INPUT_REG:
      case FUNC_WR_SGL_REG:
        s_MbMsgRx.PDU.u8DataLen = 4;
        break;

      case FUNC_WR_MULT_REGS:
        break;
      }
      s_MbMsgRx.PDU.pu8Data[u8Len-2] = u8Data;
      break;

    default :
      switch(s_MbMsgRx.PDU.u8Func)
      {
      case FUNC_RD_HOLD_REG:
      case FUNC_RD_INPUT_REG:
      case FUNC_WR_SGL_REG:
        switch(u8Len)
        {
        case 3:
        case 4:
        case 5:
          s_MbMsgRx.PDU.pu8Data[u8Len-2] = u8Data;
          break;
        
        case 6: //CRC LSB
          s_MbMsgRx.u16CRC = u8Data<<8;
          break;
        case 7: //CRC MSB
          s_MbMsgRx.u16CRC |= u8Data;
          if(s_MbMsgRx.u16CRC==s_u16CrcCal(&s_MbMsgRx))
            flgMsgOk = 1;
          else
            s_RxBuf.u8Head = s_RxBuf.u8Idx+1;
          break;
        }
        break;

      case FUNC_WR_MULT_REGS:
        switch(u8Len)
        {
        case 3:
        case 4:
        case 5:
          s_MbMsgRx.PDU.pu8Data[u8Len-2] = u8Data;
          break;
        
        case 6:
          s_MbMsgRx.PDU.u8DataLen = u8Data+5;
          s_MbMsgRx.PDU.pu8Data[u8Len-2] = u8Data;
          break;

        default:
          if(u8Len==s_MbMsgRx.PDU.u8DataLen+2)
            s_MbMsgRx.u16CRC = u8Data<<8;
          else if(u8Len==s_MbMsgRx.PDU.u8DataLen+3)
          {
            s_MbMsgRx.u16CRC |= u8Data;
            if(s_MbMsgRx.u16CRC==s_u16CrcCal(&s_MbMsgRx))
              flgMsgOk = 1;
            else
              s_RxBuf.u8Head = s_RxBuf.u8Idx+1;
          }
          else if(u8Len<s_MbMsgRx.PDU.u8DataLen+2)
            s_MbMsgRx.PDU.pu8Data[u8Len-2] = u8Data;
          break;
        }
        break;
      }
      break;
    }

    s_RxBuf.u8Idx++;
  }

  if(1==flgMsgOk) //接收完成,刷新buf
  {
    s_RxBuf.u8Head = s_RxBuf.u8Idx;
  }

  return flgMsgOk;
}

/********************************************************************************
*函数功能: 
  发送中断
*修改记录: 
*日期:                作者:                摘要:
      21-03-21             YummoShin            Create
********************************************************************************/
void ISR_BufTx(UART_HandleTypeDef *huart)
{
  if(huart==&huart6)
  {
    s_TxBuf.u8Head++;
    if(s_TxBuf.u8Head!=s_TxBuf.u8Tail)
    {
      huart->Instance->TDR = s_TxBuf.u8Buf[s_TxBuf.u8Head];
    }
    else
    {
      __HAL_UART_DISABLE_IT(huart, UART_IT_TXE);
    }
  }
}

/********************************************************************************
*函数功能: 
  接收中断
*修改记录: 
*日期:                作者:                摘要:
      21-03-21             YummoShin            Create
********************************************************************************/
void ISR_BufRx(UART_HandleTypeDef *huart)
{
  if(huart==&huart6)
  {
    s_RxBuf.u8Buf[s_RxBuf.u8Tail] = huart->Instance->RDR;
    s_RxBuf.u8Tail++;
    if(s_RxBuf.u8Tail==s_RxBuf.u8Head)
    {
      s_RxBuf.u8Head++;
      s_RxBuf.u8Idx = s_RxBuf.u8Head;
    }
  }
}

/*******************************************************************************
*反转CRC16ModBus表，实际MSB为低8位，LSB为高8位。为配合正序CRC16算法，高低位互换
*ModBus数据流中：
*
*Data[n],              Data[n-1],            Data[n-2],...,Data[1],Data[0]
*CRC16Hi(CRCtableLSB), CRC16Lo(CRCtableMSB),...
*
*从Data[0]开始读取先读低位至Data[n]。
*按正序字节型CRC16算法，相当于Data[n-1]为高8位，Data[n]为低8位。
*******************************************************************************/
const uint8_t CRCtableMSB[]=
{
	0x00,0xc1,0x81,0x40,0x01,0xc0,0x80,0x41,0x01,0xc0,
	0x80,0x41,0x00,0xc1,0x81,0x40,0x01,0xc0,0x80,0x41,
	0x00,0xc1,0x81,0x40,0x00,0xc1,0x81,0x40,0x01,0xc0,
	0x80,0x41,0x01,0xc0,0x80,0x41,0x00,0xc1,0x81,0x40,
	0x00,0xc1,0x81,0x40,0x01,0xc0,0x80,0x41,0x00,0xc1,
	0x81,0x40,0x01,0xc0,0x80,0x41,0x01,0xc0,0x80,0x41,
	0x00,0xc1,0x81,0x40,0x01,0xc0,0x80,0x41,0x00,0xc1,
	0x81,0x40,0x00,0xc1,0x81,0x40,0x01,0xc0,0x80,0x41,
	0x00,0xc1,0x81,0x40,0x01,0xc0,0x80,0x41,0x01,0xc0,
	0x80,0x41,0x00,0xc1,0x81,0x40,0x00,0xc1,0x81,0x40,
	0x01,0xc0,0x80,0x41,0x01,0xc0,0x80,0x41,0x00,0xc1,
	0x81,0x40,0x01,0xc0,0x80,0x41,0x00,0xc1,0x81,0x40,
	0x00,0xc1,0x81,0x40,0x01,0xc0,0x80,0x41,0x01,0xc0,
	0x80,0x41,0x00,0xc1,0x81,0x40,0x00,0xc1,0x81,0x40,
	0x01,0xc0,0x80,0x41,0x00,0xc1,0x81,0x40,0x01,0xc0,
	0x80,0x41,0x01,0xc0,0x80,0x41,0x00,0xc1,0x81,0x40,
	0x00,0xc1,0x81,0x40,0x01,0xc0,0x80,0x41,0x01,0xc0,
	0x80,0x41,0x00,0xc1,0x81,0x40,0x01,0xc0,0x80,0x41,
	0x00,0xc1,0x81,0x40,0x00,0xc1,0x81,0x40,0x01,0xc0,
	0x80,0x41,0x00,0xc1,0x81,0x40,0x01,0xc0,0x80,0x41,
	0x01,0xc0,0x80,0x41,0x00,0xc1,0x81,0x40,0x01,0xc0,
	0x80,0x41,0x00,0xc1,0x81,0x40,0x00,0xc1,0x81,0x40,
	0x01,0xc0,0x80,0x41,0x01,0xc0,0x80,0x41,0x00,0xc1,
	0x81,0x40,0x00,0xc1,0x81,0x40,0x01,0xc0,0x80,0x41,
	0x00,0xc1,0x81,0x40,0x01,0xc0,0x80,0x41,0x01,0xc0,
	0x80,0x41,0x00,0xc1,0x81,0x40
};

const uint8_t CRCtableLSB[]=
{
	0x00,0xc0,0xc1,0x01,0xc3,0x03,0x02,0xc2,0xc6,0x06,
	0x07,0xc7,0x05,0xc5,0xc4,0x04,0xcc,0x0c,0x0d,0xcd,
	0x0f,0xcf,0xce,0x0e,0x0a,0xca,0xcb,0x0b,0xc9,0x09,
	0x08,0xc8,0xd8,0x18,0x19,0xd9,0x1b,0xdb,0xda,0x1a,
	0x1e,0xde,0xdf,0x1f,0xdd,0x1d,0x1c,0xdc,0x14,0xd4,
	0xd5,0x15,0xd7,0x17,0x16,0xd6,0xd2,0x12,0x13,0xd3,
	0x11,0xd1,0xd0,0x10,0xf0,0x30,0x31,0xf1,0x33,0xf3,
	0xf2,0x32,0x36,0xf6,0xf7,0x37,0xf5,0x35,0x34,0xf4,
	0x3c,0xfc,0xfd,0x3d,0xff,0x3f,0x3e,0xfe,0xfa,0x3a,
	0x3b,0xfb,0x39,0xf9,0xf8,0x38,0x28,0xe8,0xe9,0x29,
	0xeb,0x2b,0x2a,0xea,0xee,0x2e,0x2f,0xef,0x2d,0xed,
	0xec,0x2c,0xe4,0x24,0x25,0xe5,0x27,0xe7,0xe6,0x26,
	0x22,0xe2,0xe3,0x23,0xe1,0x21,0x20,0xe0,0xa0,0x60,
	0x61,0xa1,0x63,0xa3,0xa2,0x62,0x66,0xa6,0xa7,0x67,
	0xa5,0x65,0x64,0xa4,0x6c,0xac,0xad,0x6d,0xaf,0x6f,
	0x6e,0xae,0xaa,0x6a,0x6b,0xab,0x69,0xa9,0xa8,0x68,
	0x78,0xb8,0xb9,0x79,0xbb,0x7b,0x7a,0xba,0xbe,0x7e,
	0x7f,0xbf,0x7d,0xbd,0xbc,0x7c,0xb4,0x74,0x75,0xb5,
	0x77,0xb7,0xb6,0x76,0x72,0xb2,0xb3,0x73,0xb1,0x71,
	0x70,0xb0,0x50,0x90,0x91,0x51,0x93,0x53,0x52,0x92,
	0x96,0x56,0x57,0x97,0x55,0x95,0x94,0x54,0x9c,0x5c,
	0x5d,0x9d,0x5f,0x9f,0x9e,0x5e,0x5a,0x9a,0x9b,0x5b,
	0x99,0x59,0x58,0x98,0x88,0x48,0x49,0x89,0x4b,0x8b,
	0x8a,0x4a,0x4e,0x8e,0x8f,0x4f,0x8d,0x4d,0x4c,0x8c,
	0x44,0x84,0x85,0x45,0x87,0x47,0x46,0x86,0x82,0x42,
	0x43,0x83,0x41,0x81,0x80,0x40
};

/********************************************************************************
*函数功能: Modbus CRC 校验，字节型算法，返回mosbus CRC16高低互换
*修改记录:
*日期:                作者:                摘要:
      20-04-27             YummoShin            Create
********************************************************************************/
static uint16_t s_u16CrcCal(MbAdu_t *pADU)
{
  uint8_t i = 0, idx = 0;
  uint8_t CrcHi = 0xFF;		//CRC初值，字节型算法名义上的高位
  uint8_t CrcLo = 0xFF;		//CRC初值，字节型算法名义上的低位
  uint16_t CRC16;

  //地址字节
  idx = CrcHi^(pADU->u8Adr);
  CrcHi = CrcLo^(CRCtableMSB[idx]);
  CrcLo = CRCtableLSB[idx];
  //功能码
  idx = CrcHi^(pADU->PDU.u8Func);
  CrcHi = CrcLo^(CRCtableMSB[idx]);
  CrcLo = CRCtableLSB[idx];
  //PDU域中Data
  for(i=0; i<(pADU->PDU.u8DataLen); i++)
  {
    idx = CrcHi^((pADU->PDU.pu8Data)[i]);
    CrcHi = CrcLo^(CRCtableMSB[idx]);
    CrcLo = CRCtableLSB[idx];
  }
	//因为CRC table 是高低位互换的，
	//故CrcHi对应ModbusCRC16低位，CrcLo对应ModbusCRC16高位
	//返回对应modbusRTU格式的CRC16低位在前、高位在后
  CRC16 = (CrcHi<<8)+CrcLo; 

  return CRC16;
}

/********************************************************************************
*函数功能: 
  解析接收帧,执行相应操作并生成相应回复帧
*修改记录: 
*日期:                作者:                摘要:
      21-03-22             YummoShin            Create
********************************************************************************/
static void s_MsgUnpack(void)
{
  uint16_t u16SReg, u16NReg;
  uint16_t i;

  switch(s_MbMsgRx.PDU.u8Func)
  {
  case FUNC_RD_HOLD_REG:
    u16SReg = s_MbMsgRx.PDU.pu8Data[0]<<8|s_MbMsgRx.PDU.pu8Data[1];
    u16NReg = s_MbMsgRx.PDU.pu8Data[2]<<8|s_MbMsgRx.PDU.pu8Data[3];
    s_MbMsgTx.u8Adr = SLAVE_ADDR;
    s_MbMsgTx.PDU.u8Func = FUNC_RD_HOLD_REG;

    if(u16SReg+u16NReg<=HOLD_REG_SIZE)
    {
      s_MbMsgTx.PDU.u8DataLen = u16NReg*2+1;
      s_MbMsgTx.PDU.pu8Data[0] = s_MbMsgTx.PDU.u8DataLen-1;
      for(i=0; i<u16NReg; i++)
      {
        s_MbMsgTx.PDU.pu8Data[2*i+1] = *(HoldReg[u16SReg+i].pu16DataPtr)>>8;
        s_MbMsgTx.PDU.pu8Data[2*i+2] = *(HoldReg[u16SReg+i].pu16DataPtr);
      }
    }
    else
    {
      s_MbMsgTx.PDU.u8Func |= 0x80;
      s_MbMsgTx.PDU.u8DataLen = 1;
      s_MbMsgTx.PDU.pu8Data[0] = CODE_ILGAL_DATA_ADR;
    }
    break;

  case FUNC_RD_INPUT_REG:
    u16SReg = s_MbMsgRx.PDU.pu8Data[0]<<8|s_MbMsgRx.PDU.pu8Data[1];
    u16NReg = s_MbMsgRx.PDU.pu8Data[2]<<8|s_MbMsgRx.PDU.pu8Data[3];
    s_MbMsgTx.u8Adr = SLAVE_ADDR;
    s_MbMsgTx.PDU.u8Func = FUNC_RD_INPUT_REG;
    if(u16SReg+u16NReg<=INPUT_REG_SIZE)
    {
      s_MbMsgTx.PDU.u8DataLen = u16NReg*2+1;
      s_MbMsgTx.PDU.pu8Data[0] = s_MbMsgTx.PDU.u8DataLen-1;
      for(i=0; i<u16NReg; i++)
      {
        s_MbMsgTx.PDU.pu8Data[2*i+1] = *(InputReg[u16SReg+i].pu16DataPtr)>>8;
        s_MbMsgTx.PDU.pu8Data[2*i+2] = *(InputReg[u16SReg+i].pu16DataPtr);
      }
    }
    else
    {
      s_MbMsgTx.PDU.u8Func |= 0x80;
      s_MbMsgTx.PDU.u8DataLen = 1;
      s_MbMsgTx.PDU.pu8Data[0] = CODE_ILGAL_DATA_ADR;
    }
    break;

  case FUNC_WR_SGL_REG:
    u16SReg = s_MbMsgRx.PDU.pu8Data[0]<<8|s_MbMsgRx.PDU.pu8Data[1];
    s_MbMsgTx.u8Adr = SLAVE_ADDR;
    s_MbMsgTx.PDU.u8Func = FUNC_WR_SGL_REG;
    s_MbMsgTx.PDU.u8DataLen = 4;
    if(u16SReg<=HOLD_REG_SIZE)
    {
      *(HoldReg[u16SReg].pu16DataPtr) = s_MbMsgRx.PDU.pu8Data[2]<<8|s_MbMsgRx.PDU.pu8Data[3];
      s_MbMsgTx.PDU.u8DataLen = 4; //data内容一样,指针一样
      g_flagModbusValChange = 1;
    }
    else
    {
      s_MbMsgTx.PDU.u8Func |= 0x80;
      s_MbMsgTx.PDU.u8DataLen = 1;
      s_MbMsgTx.PDU.pu8Data[0] = CODE_ILGAL_DATA_ADR;
    }
    break;

  case FUNC_WR_MULT_REGS:
    u16SReg = s_MbMsgRx.PDU.pu8Data[0]<<8|s_MbMsgRx.PDU.pu8Data[1];
    u16NReg = s_MbMsgRx.PDU.pu8Data[2]<<8|s_MbMsgRx.PDU.pu8Data[3];
    s_MbMsgTx.u8Adr = SLAVE_ADDR;
    s_MbMsgTx.PDU.u8Func = FUNC_WR_MULT_REGS;
    if(u16SReg+u16NReg<=HOLD_REG_SIZE)
    {
      for(i=0; i<u16NReg; i++)
      {
        *(HoldReg[u16SReg+i].pu16DataPtr) = s_MbMsgRx.PDU.pu8Data[2*i+5]<<8|s_MbMsgRx.PDU.pu8Data[2*i+6];
      }
      s_MbMsgTx.PDU.u8DataLen = 4; //data内容 指针一样
      g_flagModbusValChange = 1;
    }
    else
    {
      s_MbMsgTx.PDU.u8Func |=0x80;
      s_MbMsgTx.PDU.u8DataLen = 1;
      s_MbMsgTx.PDU.pu8Data[0] = CODE_ILGAL_DATA_ADR;
    }
    break;
  }

  s_MbMsgTx.u16CRC = s_u16CrcCal(&s_MbMsgTx);
}

/********************************************************************************
*函数功能: 
  打包发送帧,并启动发送
*修改记录: 
*日期:                作者:                摘要:
      21-03-25             YummoShin            Create
********************************************************************************/
static void s_MsgReqsonse(void)
{
  uint8_t i;

  s_TxBuf.u8Buf[s_TxBuf.u8Tail] = s_MbMsgTx.u8Adr;
  s_TxBuf.u8Tail++;
  s_TxBuf.u8Buf[s_TxBuf.u8Tail] = s_MbMsgTx.PDU.u8Func;
  s_TxBuf.u8Tail++;
  for(i=0; i<s_MbMsgTx.PDU.u8DataLen; i++)
  {
    s_TxBuf.u8Buf[s_TxBuf.u8Tail] = s_MbMsgTx.PDU.pu8Data[i];
    s_TxBuf.u8Tail++;
  }
  s_TxBuf.u8Buf[s_TxBuf.u8Tail] = s_MbMsgTx.u16CRC>>8;
  s_TxBuf.u8Tail++;
  s_TxBuf.u8Buf[s_TxBuf.u8Tail] = s_MbMsgTx.u16CRC;
  s_TxBuf.u8Tail++;

  huart6.Instance->TDR = s_TxBuf.u8Buf[s_TxBuf.u8Head];
  __HAL_UART_ENABLE_IT(&huart6, UART_IT_TXE);
}
